#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# C++ version Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
# Python version by Ken Lauer / sirkne at gmail dot com
#
# This software is provided 'as-is', without any express or implied
# warranty.  In no event will the authors be held liable for any damages
# arising from the use of this software.
# Permission is granted to anyone to use this software for any purpose,
# including commercial applications, and to alter it and redistribute it
# freely, subject to the following restrictions:
# 1. The origin of this software must not be misrepresented; you must not
# claim that you wrote the original software. If you use this software
# in a product, an acknowledgment in the product documentation would be
# appreciated but is not required.
# 2. Altered source versions must be plainly marked as such, and must not be
# misrepresented as being the original software.
# 3. This notice may not be removed or altered from any source distribution.
import pygame

from framework import (Framework, Keys, main)
from Box2D import (b2EdgeShape, b2FixtureDef, b2PolygonShape, b2_dynamicBody,
                   b2_kinematicBody, b2_staticBody)


class BodyTypes(Framework):
    name = ""
    # description = "Change body type keys: (d) dynamic, (s) static, (k) kinematic"
    speed = 3  # platform speed

    def __init__(self):
        super(BodyTypes, self).__init__()
        self.using_contacts = True
        # self.world.gravity = (0.0, 0.0)
        self.attachmentLeft = self.world.CreateDynamicBody(
            position=(-25, 10),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(1, 10)), density=2.0)
        )
        self.piece2 = self.world.CreateDynamicBody(
            position=(-16, 19),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(10, 1)), density=1.0, friction=1)
        )
        # self.piece3 = self.world.CreateDynamicBody(
        #     position=(-7, 16),
        #     fixtures=b2FixtureDef(shape=b2PolygonShape(box=(0, 0)), density=1.0, friction=1)
        # )
        self.piece4 = self.world.CreateDynamicBody(
            position=(-7, 17),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(8, 1)), density=1.0, friction=1)
        )
        self.ground_body = self.world.CreateStaticBody(
            position=(0, 0),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0)), density=2.0)
        )
        self.platform = self.world.CreateStaticBody(
            position=(0, 0.5),
            fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0.5)), density=20, friction=0.6)
        )
        # self.world.CreatePrismaticJoint(
        #     bodyA=self.attachmentLeft,
        #     bodyB=self.platform,
        #     anchor=(0, 0.5),
        #     maxMotorTorque=0,
        #     enableMotor=True
        # )
        self.world.CreateWeldJoint(
            bodyA=self.attachmentLeft,
            bodyB=self.platform,
            anchor=(-25, 0.5)
        )
        # self.world.CreatePrismaticJoint(
        #     bodyA=self.attachmentLeft,
        #     bodyB=self.platform,
        #     anchor=(-25, 0.5),
        #     axis=(1, 0),
        #     maxMotorForce=1000,
        #     enableMotor=True,
        #     lowerTranslation=0,
        #     upperTranslation=0,
        #     enableLimit=False
        # )
        self.world.CreateWeldJoint(
            bodyA=self.attachmentLeft,
            bodyB=self.piece2,
            anchor=(-25, 19)
        )
        # self.world.CreatePrismaticJoint(
        #     bodyA=self.attachmentLeft,
        #     bodyB=self.piece2,
        #     anchor=(-25, 19),
        #     axis=(1, 0),
        #     maxMotorForce=1000,
        #     enableMotor=False,
        #     lowerTranslation=0,
        #     upperTranslation=0,
        #     enableLimit=True
        # )
        # self.world.CreateWeldJoint(
        #     bodyA=self.piece2,
        #     bodyB=self.piece3,
        #     anchor=(-7, 19)
        # )
        # self.world.CreatePrismaticJoint(
        #     bodyA=self.piece2,
        #     bodyB=self.piece3,
        #     anchor=(-7, 19),
        #     axis=(1, 0),
        #     maxMotorForce=1000,
        #     enableMotor=False,
        #     lowerTranslation=0,
        #     upperTranslation=0,
        #     enableLimit=True
        # )
        self.world.CreatePrismaticJoint(
            bodyA=self.piece2,
            bodyB=self.piece4,
            anchor=(-7, 19),
            axis=(0, 1),
            maxMotorForce=1000,
            enableMotor=False,
            lowerTranslation=-5,
            upperTranslation=0,
            enableLimit=True
        )
        # self.payload = self.world.CreateStaticBody(
        #     position=(0, 20),
        #     fixtures=b2FixtureDef(shape=b2PolygonShape(box=(50, 0.8)), density=1.0, friction=1)
        # )
        # self.leftAttachmentStatus = None  # 0: closed, 1: open, 2: grabbing something
        # self.rightAttachmentStatus = None

    def Keyboard(self, key):
        if key == Keys.K_q:  # open the gripper
            if self.leftAttachmentStatus == 1:
                return
            fl = self.attachmentLeft.GetWorldVector(localVector=(-1000.0, 0))
            pl = self.attachmentLeft.GetWorldPoint(localPoint=(0, 0))
            self.attachmentLeft.ApplyForce(fl, pl, True)
            if self.rightAttachmentStatus == 1:
                return
            fr = self.attachmentRight.GetWorldVector(localVector=(1000, 0.0))
            pr = self.attachmentRight.GetWorldPoint(localPoint=(0.0, 0.0))
            self.attachmentRight.ApplyForce(fr, pr, True)
        elif key == Keys.K_e:  # close the gripper
            if self.leftAttachmentStatus == 0 or self.leftAttachmentStatus == 2:
                return
            fl = self.attachmentLeft.GetWorldVector(localVector=(1.0, 0))
            pl = self.attachmentLeft.GetWorldPoint(localPoint=(0, 0))
            self.attachmentLeft.ApplyForce(fl, pl, True)
            if self.rightAttachmentStatus == 0 or self.rightAttachmentStatus == 2:
                return
            fr = self.attachmentRight.GetWorldVector(localVector=(-1000, 0.0))
            pr = self.attachmentRight.GetWorldPoint(localPoint=(0.0, 0.0))
            self.attachmentRight.ApplyForce(fr, pr, True)
        elif key == Keys.K_s:
            fl = self.piece4.GetWorldVector(localVector=(0, -2000))
            pl = self.piece4.GetWorldPoint(localPoint=(0, 0))
            self.piece4.ApplyForce(fl, pl, True)
        elif key == Keys.K_w:
            fl = self.piece4.GetWorldVector(localVector=(0, 5000))
            pl = self.piece4.GetWorldPoint(localPoint=(0, 0))
            self.piece4.ApplyForce(fl, pl, True)


    def Step(self, settings):
        super(BodyTypes, self).Step(settings)
        # body_pairs = [(p['fixtureA'].body, p['fixtureB'].body) for p in self.points]
        # for ip, (body1, body2) in enumerate(body_pairs):
        #     print(ip)
        # posAttachment = self.attachmentLeft.transform.position
        # posPlatform = self.platform.position
        # posPlatformLeft = posPlatform.x - 4
        # posPlatformLeftCener = posPlatform.x - 1
        # posAttachmentLeft = posAttachment.x - 0.25
        # attachmentLeftMovingDirection = None
        # if self.attachmentLeft.linearVelocity[0] > 0:
        #     attachmentLeftMovingDirection = 'right'
        # elif self.attachmentLeft.linearVelocity[0] < 0:
        #     attachmentLeftMovingDirection = 'left'
        # if attachmentLeftMovingDirection == 'left' and posAttachmentLeft <= posPlatformLeft:
        #     self.attachmentLeft.linearVelocity = (0, 0)
        #     self.leftAttachmentStatus = 1  # is open
        # if attachmentLeftMovingDirection == 'right' and posAttachmentLeft >= posPlatformLeftCener:
        #     self.attachmentLeft.linearVelocity = (0, 0)
        #     self.leftAttachmentStatus = 0  # is closed
        # # for right attachment
        # posAttachment = self.attachmentRight.transform.position
        # posPlatform = self.platform.position
        # posPlatformRight = posPlatform.x + 4
        # posPlatformRightCener = posPlatform.x + 1
        # posAttachmentRight = posAttachment.x + 0.25
        # attachmentRightMovingDirection = None
        # if self.attachmentRight.linearVelocity[0] > 0:
        #     attachmentRightMovingDirection = 'right'
        # elif self.attachmentRight.linearVelocity[0] < 0:
        #     attachmentRightMovingDirection = 'left'
        # if attachmentRightMovingDirection == 'left' and posAttachmentRight <= posPlatformRightCener:
        #     self.attachmentRight.linearVelocity = (0, 0)
        #     self.rightAttachmentStatus = 0  # is closed
        # if attachmentRightMovingDirection == 'right' and posAttachmentRight >= posPlatformRight:
        #     self.attachmentRight.linearVelocity = (0, 0)
        #     self.rightAttachmentStatus = 1  # is open
        # pygame.image.save(self.screen, 'outputImages/temp.png')


if __name__ == "__main__":
    main(BodyTypes)
